#include "pca9685.h"

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <math.h>

// 往一个寄存器里面写一个数据
static void cmd_send(uint8_t addr, uint8_t reg, uint8_t value)
{
	uint8_t cmd[100] = {0};
	
	sprintf(cmd ,"i2cset -f -y %d 0x%02x 0x%02x 0x%02x" , \
			IIC_BUS_INDEX, addr, reg, value);
	
	system(cmd);
}

// 往连续的寄存器里面各写一个字节的数据
static void cmds_send(uint8_t addr, uint8_t reg, \
			uint8_t value1, uint8_t value2, \
			uint8_t value3, uint8_t value4)
{
	uint8_t cmd[100] = {0};
	
	sprintf(cmd ,"i2cset -f -y %d 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x i" , \
		IIC_BUS_INDEX, addr, reg, value1, value2, value3, value4);
	
	system(cmd);
}

// 占空比设置
void pca9685_pwm_duty_set(uint8_t channel, uint32_t on, uint32_t off)
{
	cmds_send(PCA9685_I2C_ADDR, LED0_ON_L+4*channel, on&0xff, (on>>8)&0xff, off&0xff, (off>>8)&0xff);
}

// 设置频率
void pca9685_frequency_set(uint32_t hz)
{
	uint8_t value = 0;
	
	value = round(25000000/(4095*hz))-1;
	
	cmd_send(PCA9685_I2C_ADDR, MODE1, 0x10);
	cmd_send(PCA9685_I2C_ADDR, PRE_SCALE, value);
	cmd_send(PCA9685_I2C_ADDR, MODE1, 0x00);
	cmd_send(PCA9685_I2C_ADDR, MODE1, 0xa1);
}

// 初始化PWM
void pca9685_init_pwm(void)
{
	cmd_send(PCA9685_I2C_ADDR, MODE1, 0x21);
	cmd_send(PCA9685_I2C_ADDR, MODE2, 0x04);
	cmd_send(PCA9685_I2C_ADDR, SUBADR1, 0xe2);
	cmd_send(PCA9685_I2C_ADDR, SUBADR2, 0xe4);
	cmd_send(PCA9685_I2C_ADDR, SUBADR3, 0xe8);
	cmd_send(PCA9685_I2C_ADDR, ALLCALLADR, 0xe0);
}

// 通道使能
void pca9685_pwm_enable(uint8_t channel, bool state)
{
	if(state == true)
	{
		cmd_send(PCA9685_I2C_ADDR, LED0_OFF_H+4*channel, 0x10);
	}
	else
	{
		cmd_send(PCA9685_I2C_ADDR, LED0_OFF_H+4*channel, 0x00);
	}
}

// 舵机设置角度
void pca9685_sg90_angle_set(uint8_t channel, uint8_t angle) 
{
	pca9685_pwm_duty_set(channel, 0, (uint32_t)(4095*((angle/180.0)*2.0+0.5)/20.0));
}

// 直流电机设置占空比
void pca9685_dcmotor_duty_set(uint8_t channel, uint8_t duty)
{
	pca9685_pwm_duty_set(channel, 0, (uint32_t)(4095*(duty/100.0)));
}

// 控制
void pca9685_car_control(uint8_t go, uint8_t turn, uint8_t speed)
{
	uint8_t duty = speed;
	// 全速直行
	
	if(go == GO_FORWARD) // 前进
	{
		switch(turn)
		{
		case TURN_STRAIGHT:  // 直行
		{
			// 正转
			pca9685_pwm_duty_set(0, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(1, 0, 0);
			
			pca9685_pwm_duty_set(2, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(3, 0, 0);
			
			pca9685_pwm_duty_set(4, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(5, 0, 0);
			
			pca9685_pwm_duty_set(6, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(7, 0, 0);	
		}break;
		case TURN_RIGHT: // 右转
		{
			// 正转
			pca9685_pwm_duty_set(0, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(1, 0, 0);
			
			pca9685_pwm_duty_set(2, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(3, 0, 0);
			
			pca9685_pwm_duty_set(4, 0, (uint32_t)(4095*(duty/100.0))-10);
			pca9685_pwm_duty_set(5, 0, 0);
			
			pca9685_pwm_duty_set(6, 0, (uint32_t)(4095*(duty/100.0))-10);
			pca9685_pwm_duty_set(7, 0, 0);
		}break;
		case TURN_LEFT:  // 左转
		{
			// 正转
			pca9685_pwm_duty_set(0, 0, (uint32_t)(4095*(duty/100.0))-10);
			pca9685_pwm_duty_set(1, 0, 0);
			
			pca9685_pwm_duty_set(2, 0, (uint32_t)(4095*(duty/100.0))-10);
			pca9685_pwm_duty_set(3, 0, 0);
			
			pca9685_pwm_duty_set(4, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(5, 0, 0);
			
			pca9685_pwm_duty_set(6, 0, (uint32_t)(4095*(duty/100.0)));
			pca9685_pwm_duty_set(7, 0, 0);
		}break;
		default:
			break;
		}
	}
	else if(go == GO_BACKWARD) // 后退
	{
		switch(turn)
		{
		case TURN_STRAIGHT:  // 直行
		{
			// 反转
			pca9685_pwm_duty_set(0, 0, 0);
			pca9685_pwm_duty_set(1, 0, (uint32_t)(4095*(duty/100.0)));
			
			pca9685_pwm_duty_set(2, 0, 0);
			pca9685_pwm_duty_set(3, 0, (uint32_t)(4095*(duty/100.0)));
			
			pca9685_pwm_duty_set(4, 0, 0);
			pca9685_pwm_duty_set(5, 0, (uint32_t)(4095*(duty/100.0)));
			
			pca9685_pwm_duty_set(6, 0, 0);
			pca9685_pwm_duty_set(7, 0, (uint32_t)(4095*(duty/100.0)));
	
		}break;
		case TURN_RIGHT: // 右转
		{
			// 反转
			pca9685_pwm_duty_set(0, 0, 0);
			pca9685_pwm_duty_set(1, 0, (uint32_t)(4095*(duty/100.0)));
			
			pca9685_pwm_duty_set(2, 0, 0);
			pca9685_pwm_duty_set(3, 0, (uint32_t)(4095*(duty/100.0)));
			
			pca9685_pwm_duty_set(4, 0, 0);
			pca9685_pwm_duty_set(5, 0, (uint32_t)(4095*(duty/100.0))-10);
			
			pca9685_pwm_duty_set(6, 0, 0);
			pca9685_pwm_duty_set(7, 0, (uint32_t)(4095*(duty/100.0))-10);

		}break;
		case TURN_LEFT:  // 左转
		{
			// 反转
			pca9685_pwm_duty_set(0, 0, 0);
			pca9685_pwm_duty_set(1, 0, (uint32_t)(4095*(duty/100.0))-10);
			
			pca9685_pwm_duty_set(2, 0, 0);
			pca9685_pwm_duty_set(3, 0, (uint32_t)(4095*(duty/100.0))-10);
			
			pca9685_pwm_duty_set(4, 0, 0);
			pca9685_pwm_duty_set(5, 0, (uint32_t)(4095*(duty/100.0)));
			
			pca9685_pwm_duty_set(6, 0, 0);
			pca9685_pwm_duty_set(7, 0, (uint32_t)(4095*(duty/100.0)));
		}break;
		default:
			break;
		}
	}
}

